#include"MapBuildeApplication.h"
#include"Config.h"
#include<string>
#include"LogCat.h"
#include <boost/thread/thread.hpp>
#include<pcl/filters/voxel_grid.h>
using namespace std;
#define TIME_USING to_string((int)(end_time-start_time)*1000/CLOCKS_PER_SEC)

MapBuildeApplication::MapBuildeApplication()
{

}


MapBuildeApplication::~MapBuildeApplication()
{
}


void MapBuildeApplication::appInit()
{
	TRY_START

	//read the Config.ini file
	//---------------------------------
	if (!mConfig.ReadConfig(CONFIG_FILE_PATH))
		throw "there can not find config.ini file!";
	mconfig.interval = mConfig.ReadInt("MAPPING PARA", "interval", 0);
	mconfig.max_iterate = mConfig.ReadInt("MAPPING PARA", "max_iterate", 0);
	mconfig.width_of_road = mConfig.ReadFloat("MAPPING PARA", "width_of_road", 0.0);

	mconfig._sequence= mConfig.ReadString("INPUT_OUTPUT", "SEQUENCE", "");
	mconfig.bin_input_path= mConfig.ReadString("INPUT_OUTPUT", "BIN_INPUT_PATH", "");
	mconfig.pcd_input_path = mConfig.ReadString("INPUT_OUTPUT", "PCD_INPUT_PATH", "");
	mconfig.pcd_output_path = mConfig.ReadString("INPUT_OUTPUT", "PCD_OUTPUT_PATH", "");
	mconfig.pcd_map_path = mConfig.ReadString("INPUT_OUTPUT", "PCD_MAP_PATH", "");
	mconfig.bin_map_path = mConfig.ReadString("INPUT_OUTPUT", "BIN_MAP_PATH", "");
			 
	mconfig.pose_data_path = mConfig.ReadString("INPUT_OUTPUT", "POSE_DATA_PATH", "");
	mconfig.logcat_path = mConfig.ReadString("INPUT_OUTPUT", "LOGCAT_PATH", "");

	mconfig._iterate = mConfig.ReadInt("RUN", "iterate", 0);
	mconfig.segicp = mConfig.ReadInt("RUN", "segicp", 0);
	mconfig.icp = mConfig.ReadInt("RUN", "icp", 0);
	mconfig.create_map = mConfig.ReadInt("RUN", "create_map", 0);
	mconfig.create_noisy_map = mConfig.ReadInt("RUN", "create_noisy_map", 0);
	mconfig.noisy= mConfig.ReadFloat("RUN", "noisy", 0.0);

	mconfig.show_3d = mConfig.ReadInt("SHOW_3D", "show_3d", 0);
	mconfig.show_ground_truth = mConfig.ReadInt("SHOW_3D", "show_ground_truth", 0);
	mconfig.show_ground_truth_with_noisy = mConfig.ReadInt("SHOW_3D", "show_ground_truth_with_noisy", 0);
	mconfig.color_with_z = mConfig.ReadInt("SHOW_3D", "color_with_z", 0);

	//load the Groud Truth
	//---------------------------------
	if (!mPoseGrabber.PoseDataInit(mconfig.pose_data_path))
		throw "Init ground truth fail!";

	//load the observation truth
	//---------------------------------
	if (!mObservation.observationDataInit(mconfig.bin_input_path)) throw "no obsevation file";

	//Init the mapping
	//---------------------------------
	if (!mMapping.mapDataInit(mconfig._iterate)) throw "a o";

	TRY_END
}

void MapBuildeApplication::appRun()
{
	TRY_START
	//==================================================================================
	//Initialize the cloud ptr
	//==================================================================================
	pcl::PointCloud<PointIoType>::Ptr GT_pose_cloud(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr GTNoisy_pose_cloud(new pcl::PointCloud<PointIoType>);

	pcl::PointCloud<PointIoType>::Ptr sour_SEGICP_regist(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr tar_SEGICP_regist(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr SEGICP_map(new pcl::PointCloud<PointIoType>);

	pcl::PointCloud<PointIoType>::Ptr sour_ICP_regist(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr tar_ICP_regist(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr ICP_map(new pcl::PointCloud<PointIoType>);

	pcl::PointCloud<PointIoType>::Ptr sour_Noisy(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr tar_Noisy(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr NOISY_map(new pcl::PointCloud<PointIoType>);

	pcl::PointCloud<PointIoType>::Ptr sour_pc_temp(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr tar_pc_temp(new pcl::PointCloud<PointIoType>);
	pcl::PointCloud<PointIoType>::Ptr GT_map(new pcl::PointCloud<PointIoType>);
	
	//==================================================================================
	//GT about  
	//inupt GT/GT_Noisy 
	//output GT_pose_cloud/GTNoisy_pose_cloud   AND   _Kdtree
	//==================================================================================
	pcl::KdTreeFLANN<PointIoType> _Kdtree;
	//GT_pose cloud create
	vector<Pose3D> GT = mPoseGrabber.getGroudTruth();
	GT_pose_cloud = mPoseGrabber.getPoseAsPointCloud();
	//GTNoisy_pose cloud create
	vector<Pose3D> GT_noisy(GT);
	if (mconfig.noisy != 0.0) {
		mPoseGrabber.getGroudTruthWithNoisy(GT_noisy, mconfig.noisy);
		int threshold = 40;
		pcl::PointXYZ truth_point;
		for (size_t i = 0; i < GT_noisy.size() - threshold; i++) {
			truth_point.x = GT_noisy[i].x;
			truth_point.y = GT_noisy[i].y;
			truth_point.z = GT_noisy[i].z;
			GTNoisy_pose_cloud->points.push_back(truth_point);
		}
		_Kdtree.setInputCloud(GTNoisy_pose_cloud);
	}else {
		_Kdtree.setInputCloud(GT_pose_cloud);
	}

	//==================================================================================
	//logcat about init to save and show 
	//==================================================================================
	vector<double> SEGICP_score_list;
	vector<double> ICP_score_list;
	vector<int> SEGICP_time_list;
	vector<int> ICP_time_list;

	//==================================================================================
	//timer 
	//==================================================================================
	clock_t start_time, end_time;
	int time_using=0;

	//==================================================================================
	//run config about  
	//==================================================================================
	int max_iterate;
	int interval = mconfig.interval;
	string registration_function_string;
	bool show_3d=false;
	if (mconfig.max_iterate == 0) {
		max_iterate = GT.size();
	}else {
		max_iterate = mconfig.max_iterate;
	}
	if (mconfig.show_3d != 0) {
		show_3d = true;
	}
	else {
		show_3d = false;
	}
	//==================================================================================
	//*********************************MAIN  LOOP***************************************
	//==================================================================================
	map<string, pcl::PointCloud<PointIoType>::Ptr> display_map;
	if (mconfig.segicp==1) {
		for (int i = 0, j = i + interval; j < max_iterate; i = j, j = i + interval) {
			cout << "--------------------------------SEGICP-------------------iterate = " << j << "/" << GT_noisy.size() << endl;
			//*******************************************SEGICP*****************************************
			if (i == 0) {
				tar_pc_temp->clear();
				sour_pc_temp->clear();							// the first cannot add noisy,because it is the banchmark
				tar_pc_temp = mObservation.getFromBinWithPose(i, GT[i], mconfig.bin_input_path);
				sour_pc_temp = mObservation.getFromBinWithPose(j, GT_noisy[j], mconfig.bin_input_path);
			}
			else {
				tar_pc_temp->clear();
				pcl::copyPointCloud(*sour_SEGICP_regist, *tar_pc_temp);
				sour_pc_temp->clear();
				sour_pc_temp = mObservation.getFromBinWithPose(j, GT_noisy[j], mconfig.bin_input_path);
			}
			LogCat::c("MapBuildeApplication", "before registration core/m", to_string(mMapping.getScore(tar_pc_temp, sour_pc_temp)));
			start_time = clock();
			tar_SEGICP_regist = mObservation.getFromBinWithKdtree(mconfig.width_of_road, _Kdtree, tar_pc_temp);
			sour_SEGICP_regist = mObservation.getFromBinWithKdtree(mconfig.width_of_road, _Kdtree, sour_pc_temp);
			mMapping.registeICP(tar_SEGICP_regist, sour_SEGICP_regist, sour_SEGICP_regist);
			end_time = clock();
			registration_function_string = "segicp";
			time_using = (int)(end_time - start_time) * 1000 / CLOCKS_PER_SEC;
			SEGICP_score_list.push_back(mMapping.getScore());
			SEGICP_time_list.push_back(time_using);
			//GT2[j].m_ROT4=mMapping.getTransformation()*GT[j].m_ROT4;
			//LogCat::c("MapBuildeApplication", "transformation after=\n", GT2[j].m_ROT4);
			LogCat::c("MapBuildeApplication", "executeOn with function " + registration_function_string + "/ms", time_using);
			LogCat::c("MapBuildeApplication", "after registration core/m", to_string(mMapping.getScore()));
			//SEGICP_map build
			//---------------------------------
			if (show_3d) {
				if (i == 0) {
					*SEGICP_map = *tar_SEGICP_regist + *sour_SEGICP_regist;
				}
				else {
					*SEGICP_map = *SEGICP_map + *sour_SEGICP_regist;
				}
			}
		}//end for
		display_map.insert(pair<string, pcl::PointCloud<PointIoType>::Ptr>("SEGICP_map", SEGICP_map));
		logOutPut(mconfig.logcat_path, "AS" + mconfig._sequence, "AT" + mconfig._sequence, SEGICP_score_list, SEGICP_time_list, GT_noisy);
	}
	if (mconfig.icp==1) {
		for (int i = 0, j = i + interval; j < max_iterate; i = j, j = i + interval) {
			cout << "-----------------------------ICP----------------------iterate = " << j << "/" << GT_noisy.size() << endl;
			//*******************************************ICP*****************************************
			if (i == 0) {
				tar_pc_temp->clear();
				sour_pc_temp->clear();							// the first cannot add noisy,because it is the banchmark
				tar_pc_temp = mObservation.getFromBinWithPose(i, GT[i], mconfig.bin_input_path);
				sour_pc_temp = mObservation.getFromBinWithPose(j, GT_noisy[j], mconfig.bin_input_path);
			}
			else {
				tar_pc_temp->clear();
				pcl::copyPointCloud(*sour_ICP_regist, *tar_pc_temp);
				sour_pc_temp->clear();
				sour_pc_temp = mObservation.getFromBinWithPose(j, GT_noisy[j], mconfig.bin_input_path);
			}
			LogCat::c("MapBuildeApplication", "before registration core/m", to_string(mMapping.getScore(tar_pc_temp, sour_pc_temp)));
			start_time = clock();
			tar_ICP_regist = tar_pc_temp;
			sour_ICP_regist = sour_pc_temp;
			mMapping.registeICP(tar_ICP_regist, sour_ICP_regist, sour_ICP_regist);
			end_time = clock();
			registration_function_string = "icp";
			time_using = (int)(end_time - start_time) * 1000 / CLOCKS_PER_SEC;
			ICP_score_list.push_back(mMapping.getScore());
			ICP_time_list.push_back(time_using);
			//GT2[j].m_ROT4=mMapping.getTransformation()*GT[j].m_ROT4;
			//LogCat::c("MapBuildeApplication", "transformation after=\n", GT2[j].m_ROT4);
			LogCat::c("MapBuildeApplication", "executeOn with function " + registration_function_string + "/ms", time_using);
			LogCat::c("MapBuildeApplication", "after registration core/m", to_string(mMapping.getScore()));
			//ICP_map build
			//---------------------------------
			if (show_3d) {
				if (i == 0) {
					*ICP_map = *tar_ICP_regist + *sour_ICP_regist;
				}
				else {
					*ICP_map = *ICP_map + *sour_ICP_regist;
				}
			}
		}//end for
		display_map.insert(pair<string, pcl::PointCloud<PointIoType>::Ptr>("ICP_map", ICP_map));
		logOutPut(mconfig.logcat_path,"BS"+mconfig._sequence, "BT" + mconfig._sequence,ICP_score_list, ICP_time_list, GT_noisy);
	
	}
	if (mconfig.create_noisy_map) {
		for (int i = 0, j = i + interval; j < max_iterate; i = j, j = i + interval) {
			cout << "------------------------------NOISY_GT--------------------iterate = " << j << "/" << GT_noisy.size() << endl;
			//*******************************************NOISY_GT****************************************
			if (i == 0) {
				tar_pc_temp->clear();
				sour_pc_temp->clear();	// the first cannot add noisy,because it is the banchmark
				tar_pc_temp = mObservation.getFromBinWithPose(i, GT[i], mconfig.bin_input_path);
				sour_pc_temp = mObservation.getFromBinWithPose(j, GT_noisy[j], mconfig.bin_input_path);
			}
			else {
				sour_pc_temp->clear();
				sour_pc_temp = mObservation.getFromBinWithPose(j, GT_noisy[j], mconfig.bin_input_path);
			}
			tar_Noisy = tar_pc_temp;
			sour_Noisy = sour_pc_temp;
			start_time = clock();
			mMapping.getScore(tar_Noisy, sour_Noisy);
			end_time = clock();
			registration_function_string = "null";
			//NOISY_map build
			//---------------------------------
			if (show_3d) {
				if (i == 0) {
					*NOISY_map = *tar_Noisy + *sour_Noisy;
				}
				else {
					*NOISY_map = *NOISY_map + *sour_Noisy;
				}
			}
		}//end for
		display_map.insert(pair<string, pcl::PointCloud<PointIoType>::Ptr>("NOISY_map", NOISY_map));
	}
	if (mconfig.create_map) {
		for (int i = 0, j = i + interval; j < max_iterate; i = j, j = i + interval) {
			cout << "------------------------------GT--------------------iterate = " << j << "/" << GT.size() << endl;
			//*******************************************GT****************************************
			if (i == 0) {
				tar_pc_temp->clear();
				sour_pc_temp->clear();	// the first cannot add noisy,because it is the banchmark
				tar_pc_temp = mObservation.getFromBinWithPose(i, GT[i], mconfig.bin_input_path);
				sour_pc_temp = mObservation.getFromBinWithPose(j, GT[j], mconfig.bin_input_path);
			}
			else {
				sour_pc_temp->clear();
				sour_pc_temp = mObservation.getFromBinWithPose(j, GT[j], mconfig.bin_input_path);
			}
			//GT_map build
			//---------------------------------
			if (show_3d) {
				if (i == 0) {
					*GT_map = *tar_pc_temp + *sour_pc_temp;
				}
				else {
					*GT_map = *GT_map + *sour_pc_temp;
				}
			}
		}//end for
		display_map.insert(pair<string, pcl::PointCloud<PointIoType>::Ptr>("GT_map", GT_map));
	}

	LogCat::c("MapBuildeApplication","--------------done--------------");
	//==================================================================================
	//*********************************---SHOW---ABOUT----******************************
	//==================================================================================
	if (show_3d) {
		if (display_map.size()!=0) {
			if (mconfig.show_ground_truth) {
				if (!mconfig.show_ground_truth_with_noisy) {
					show3d(viewer, display_map, GT_pose_cloud);
				}
				else {
					show3d(viewer, display_map, GTNoisy_pose_cloud);
				}
			}
			else {
				show3d(viewer, display_map, nullptr);
			}
			
		}
	}
	
	TRY_END
}


void MapBuildeApplication::logOutPut(string root, string score_name, string time_name,vector<double> score_list, vector<int> using_time,vector<Pose3D> pose_list)
{
	string score_output = root + score_name +".txt";
	ofstream scorefout;
	scorefout.open(score_output);//
	for (auto& s : score_list) {
		scorefout << s << endl;
	}
	scorefout.close();

	string time_output = root + time_name +".txt";
	ofstream timefout;
	timefout.open(time_output);//
	for (auto& t : using_time) {
		timefout << t << endl;
	}
	timefout.close();

	string pose_output = root + score_name+time_name + "_pose.txt";
	ofstream posefout;
	posefout.open(pose_output);//
	for (int i = 0; i < pose_list.size();i++) {
		posefout << pose_list[i].m_ROT4(0, 0)<<" "<< pose_list[i].m_ROT4(0, 1) << " " << pose_list[i].m_ROT4(0, 2) << " " << pose_list[i].m_ROT4(0, 3) << " "
			<< pose_list[i].m_ROT4(1, 0) << " " << pose_list[i].m_ROT4(1, 1) << " " << pose_list[i].m_ROT4(1, 2) << " " << pose_list[i].m_ROT4(1, 3) << " "
			<< pose_list[i].m_ROT4(2, 0) << " " << pose_list[i].m_ROT4(2, 1) << " " << pose_list[i].m_ROT4(2, 2) << " " << pose_list[i].m_ROT4(2, 3) << endl;
	}
	posefout.close();

}

void MapBuildeApplication::show3d(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer,
	map<string, pcl::PointCloud<PointIoType>::Ptr>& map,
	pcl::PointCloud<PointIoType>::Ptr GT)
{
	//pcl::visualization::PCLVisualizer viewer("PF Localization");
	pcl::VoxelGrid<pcl::PointXYZ> filter;
	viewer.reset(new pcl::visualization::PCLVisualizer("3D Viewer"));
	//======================================== one viewer======================================================
	if (map.size()==1) {
		viewer->addText(map.begin()->first, 10, 10, 0.1, 0.1, 0.1, "text");
		filter.setInputCloud(map.begin()->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*map.begin()->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(map.begin()->second, "z");
			viewer->addPointCloud(map.begin()->second, map_color, map.begin()->first);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(map.begin()->second, 0, 0, 250);
			viewer->addPointCloud(map.begin()->second, map_color, map.begin()->first);
		}
	}
	//======================================== two viewer======================================================
	if (map.size() == 2) {
		std::map<string, pcl::PointCloud<PointIoType>::Ptr>::iterator elment;
		elment = map.begin();
		viewer->initCameraParameters();
		int v1, v2;
		viewer->createViewPort(0.0, 0.0, 0.5, 0.1, v1); //(Xmin,Ymin,Xmax,Ymax)���ô�������
		viewer->addText(elment->first, 10, 10, 0.1, 0.1, 0.1, "v1 text", v1);
		filter.setInputCloud(elment->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*elment->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(elment->second, "z");
			viewer->addPointCloud(elment->second, map_color, elment->first, v1);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(elment->second, 0, 0, 250);
			viewer->addPointCloud(elment->second, map_color, elment->first, v1);
		}
		elment++;
		viewer->createViewPort(0.5, 0.0, 1, 1, v2); //(Xmin,Ymin,Xmax,Ymax)���ô�������
		viewer->addText(elment->first, 10, 10, 0.1, 0.1, 0.1, "v2 text", v2);
		filter.setInputCloud(elment->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*elment->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(elment->second, "z");
			viewer->addPointCloud(elment->second, map_color, elment->first, v2);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(elment->second, 0, 0, 250);
			viewer->addPointCloud(elment->second, map_color, elment->first, v2);
		}
	}
	//======================================== three viewer======================================================
	if (map.size()==3) {
		std::map<string, pcl::PointCloud<PointIoType>::Ptr>::iterator elment;
		elment = map.begin();
		viewer->initCameraParameters();
		int v1, v2,v3;
		viewer->createViewPort(0.0, 0.5, 0.5, 1, v1); //(Xmin,Ymin,Xmax,Ymax)���ô�������
		viewer->addText(elment->first, 10, 10, 0.1, 0.1, 0.1, "v1 text", v1);
		filter.setInputCloud(elment->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*elment->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(elment->second, "z");
			viewer->addPointCloud(elment->second, map_color, elment->first, v1);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(elment->second, 0, 0, 250);
			viewer->addPointCloud(elment->second, map_color, elment->first, v1);
		}
		elment++;
		viewer->createViewPort(0.5, 0.5, 1, 1, v2); //(Xmin,Ymin,Xmax,Ymax)���ô�������
		viewer->addText(elment->first, 10, 10, 0.1, 0.1, 0.1, "v2 text", v2);
		filter.setInputCloud(elment->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*elment->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(elment->second, "z");
			viewer->addPointCloud(elment->second, map_color, elment->first, v2);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(elment->second, 0, 0, 250);
			viewer->addPointCloud(elment->second, map_color, elment->first, v2);
		}
		elment++;
		viewer->createViewPort(0.0, 0.0, 1, 0.5, v3); //(Xmin,Ymin,Xmax,Ymax)���ô�������
		viewer->addText(elment->first, 10, 10, 0.1, 0.1, 0.1, "v3 text", v3);
		filter.setInputCloud(elment->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*elment->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(elment->second, "z");
			viewer->addPointCloud(elment->second, map_color, elment->first, v3);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(elment->second, 0, 0, 250);
			viewer->addPointCloud(elment->second, map_color, elment->first, v3);
		}
	}
	//=========================================four viewer======================================================
	if (map.size()==4) {
		int v1, v2, v3, v4;
		std::map<string, pcl::PointCloud<PointIoType>::Ptr>::iterator elment;
		elment = map.begin();
		viewer->initCameraParameters();
		viewer->createViewPort(0.0, 0.5, 0.5, 1, v1); //(Xmin,Ymin,Xmax,Ymax)���ô�������
		viewer->addText(elment->first, 10, 10, 0.1, 0.1, 0.1, "v1 text", v1);
		filter.setInputCloud(elment->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*elment->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(elment->second, "z");
			viewer->addPointCloud(elment->second, map_color, elment->first, v1);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(elment->second, 0, 0, 250);
			viewer->addPointCloud(elment->second, map_color, elment->first, v1);
		}
		elment++;
		viewer->createViewPort(0.5, 0.5, 1, 1, v2); //(Xmin,Ymin,Xmax,Ymax)���ô�������
		viewer->addText(elment->first, 10, 10, 0.1, 0.1, 0.1, "v2 text", v2);
		filter.setInputCloud(elment->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*elment->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(elment->second, "z");
			viewer->addPointCloud(elment->second, map_color, elment->first, v2);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(elment->second, 0, 0, 250);
			viewer->addPointCloud(elment->second, map_color, elment->first, v2);
		}
		elment++;
		viewer->createViewPort(0.0, 0.0, 0.5, 0.5, v3); //(Xmin,Ymin,Xmax,Ymax)���ô�������
		viewer->addText(elment->first, 10, 10,0.1,0.1,0.1, "v3 text", v3);
		filter.setInputCloud(elment->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*elment->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(elment->second, "z");
			viewer->addPointCloud(elment->second, map_color, elment->first, v3);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(elment->second, 0, 0, 250);
			viewer->addPointCloud(elment->second, map_color, elment->first, v3);
		}
		elment++;
		viewer->createViewPort(0.5, 0.0, 1, 0.5, v4); //(Xmin,Ymin,Xmax,Ymax)���ô�������
		viewer->addText(elment->first, 10, 10, 0.1, 0.1, 0.1, "v4 text", v4);
		filter.setInputCloud(elment->second);
		filter.setLeafSize(0.1f, 0.1f, 0.1f);
		filter.filter(*elment->second);
		if (mconfig.color_with_z) {
			//turn over the z to show ,if you want .otherwise the larger z, the lighter the color(blue)
			/*for (auto& p:map->points) {
				p.z = - p.z;
			}*/
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> map_color(elment->second, "z");
			viewer->addPointCloud(elment->second, map_color, elment->first, v4);
		}
		else {
			pcl::visualization::PointCloudColorHandlerCustom<PointIoType> map_color(elment->second, 0, 0, 250);
			viewer->addPointCloud(elment->second, map_color, elment->first, v4);
		}
	}
	if (GT) {
		pcl::visualization::PointCloudColorHandlerCustom<PointIoType> truth_pose_cloud_color(GT, 250, 0, 0);
		viewer->addPointCloud(GT, truth_pose_cloud_color, "truth_pose_cloud");
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "truth_pose_cloud");
	}
	viewer->setBackgroundColor(255, 255, 255);
	while (!viewer->wasStopped()) {
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}
